/**
 * @file mathematics.h
 * @brief mathematic functions used in transmiter
 * @author - Robert.Zhang<armner@gmail.com>
 * @date 2013-11-06
 */
#include "mathematics.h"

#define RESOLUTION (4096)

#define H (RESOLUTION/6)
#define POINT_0 (-RESOLUTION/2)
#define POINT_1 (-RESOLUTION/3)
#define POINT_2 (-RESOLUTION/6)
#define POINT_3 (0)
#define POINT_4 (RESOLUTION/6)
#define POINT_5 (RESOLUTION/3)
#define POINT_6 (RESOLUTION/2)

const float h=(RESOLUTION/6);
static const float B_2048[7][7] =
{
  { 0.01757812500000,                  0,                  0,                  0,                  0,                  0,   0.00000000000000},
  {-0.00471003605769,   0.00001379893376,  -0.00000369732197,   0.00000099035410,  -0.00000026409443,   0.00000006602361,  -0.00002253605769},
  { 0.00126201923077,  -0.00000369732197,   0.00001478928786,  -0.00000396141639,   0.00000105637770,  -0.00000026409443,   0.00009014423077},
  {-0.00033804086538,   0.00000099035410,  -0.00000396141639,   0.00001485531147,  -0.00000396141639,   0.00000099035410,  -0.00033804086538},
  { 0.00009014423077,  -0.00000026409443,   0.00000105637770,  -0.00000396141639,   0.00001478928786,  -0.00000369732197,   0.00126201923077},
  {-0.00002253605769,   0.00000006602361,  -0.00000026409443,   0.00000099035410,  -0.00000369732197,   0.00001379893376,  -0.00471003605769},
  {                0,                  0,                  0,                  0,                  0,                  0,   0.01757812500000}

};

static const float B_4096[7][7] =
{
  { 0.00878906250000,                  0,                  0,                  0,                  0,                  0,   0.00000000000000},
  {-0.00235501802885,   0.00000344973344,  -0.00000092433049,   0.00000024758852,  -0.00000006602361,   0.00000001650590,  -0.00001126802885},
  { 0.00063100961538,  -0.00000092433049,   0.00000369732197,  -0.00000099035410,   0.00000026409443,  -0.00000006602361,   0.00004507211538},
  {-0.00016902043269,   0.00000024758852,  -0.00000099035410,   0.00000371382787,  -0.00000099035410,   0.00000024758852,  -0.00016902043269},
  { 0.00004507211538,  -0.00000006602361,   0.00000026409443,  -0.00000099035410,   0.00000369732197,  -0.00000092433049,   0.00063100961538},
  {-0.00001126802885,   0.00000001650590,  -0.00000006602361,   0.00000024758852,  -0.00000092433049,   0.00000344973344,  -0.00235501802885},
  {                0,                  0,                  0,                  0,                  0,                  0,   0.00878906250000}

};

static float m[7];
/**
 * @fn curve_7pts
 * @brief get a point value in a cruve descripted by 7 points
 *      first get Cubic Spline Interpolation 
 *      then get value on the curve
 * @param[in] input: x value
 * @param[in] points: points descript a curve 
 * @return y=f(x)
 */
int curve_7pts(int input, float point[])
{
    float t_in;
    float t_float;
    int i;

    float a,b,c,d,e;
    float HH[7];
    
    HH[0] =0.0;
    HH[1] = point[2]+point[0]-2*point[1];
    HH[2] = point[3]+point[1]-2*point[2];
    HH[3] = point[4]+point[2]-2*point[3];
    HH[4] = point[5]+point[3]-2*point[4];
    HH[5] = point[6]+point[4]-2*point[5];
    HH[6] =0;
    
    t_in =input;
    if(t_in<POINT_1)
    {
        i=0;
        e =POINT_0;
    }
    else if(input<POINT_2)
    {
        i=1;
        e =POINT_1;
    }
    else if(input<POINT_3)
    {
        i=2;
        e =POINT_2;
    }
    else if(input<POINT_4)
    {
        i=3;
        e =POINT_3;
    }
    else if(input<POINT_5)
    {
        i=4;
        e =POINT_4;
    }
    else if(input<POINT_6)
    {
        i=5;
        e =POINT_5;
    }
    else
    {
        return 0;
    }

    m[i] =B_4096[i][0]*HH[0] + B_4096[i][1]*HH[1] + B_4096[i][2]*HH[2] +B_4096[i][3]*HH[3] + B_4096[i][4]*HH[4] +B_4096[i][5]*HH[5] + B_4096[i][6]*HH[6];
    m[i+1] =B_4096[i+1][0]*HH[0] + B_4096[i+1][1]*HH[1] + B_4096[i+1][2]*HH[2] +B_4096[i+1][3]*HH[3] + B_4096[i+1][4]*HH[4] +B_4096[i+1][5]*HH[5] + B_4096[i+1][6]*HH[6];
    
    a =point[i];
    b =(point[i+1] -point[i])/h  -h*m[i]/2 - (h/6)*(m[i+1] -m[i]);
    c =m[i]/2;
    d = (m[i+1] -m[i])/(6*h);

    t_in =t_in-e;
    t_float =a + b*t_in + c*t_in*t_in +d*t_in*t_in*t_in;
    
    return (int)t_float;
}


/**
 * @fn line_7pts
 * @brief get a value between two point
 */
int line_7pts(int input, float point[])
{
    float t_float;
    float t_in;

    t_in =input;
    if(input<POINT_1)
    {
        t_float =point[0] + ((t_in -POINT_0) *6*(point[1]-point[0]) )/RESOLUTION;
    }
    else if(input<POINT_2)
    {
        t_float =point[1] + ((t_in -POINT_1) *6*(point[2]-point[1]) )/RESOLUTION;
    }
    else if(input<POINT_3)
    {
        t_float =point[2] + ((t_in -POINT_2) *6*(point[3]-point[2]) )/RESOLUTION;
    }
    else if(input<POINT_4)
    {
        t_float =point[3] + ((t_in -POINT_3) *6*(point[4]-point[3]) )/RESOLUTION;
    }
    else if(input<POINT_5)
    {
        t_float =point[4] + ((t_in -POINT_4) *6*(point[5]-point[4]) )/RESOLUTION;
    }
    else if(input<POINT_6)
    {
        t_float =point[5] + ((t_in -POINT_5) *6*(point[6]-point[5]) )/RESOLUTION;
    }
    else
        {
        return 0;
        }

    return (int) t_float;
}





